/*
 * s_user_sys.c
 *
 *  Created on: Sep 4, 2020
 *      Author: hemingway
 */
#include "s_user_sys.h"

#include <stdio.h>
#include <string.h>

#include "h_cc2640r2f_define.h"

#include "h_cc2640r2f_uart.h"
//#include "h_cc2640r2f_gpio.h"
#include "h_cc2640r2f_iic.h"

#include "h_cc2640r2f_simpletime.h"


#include "flash_interface.h"
#include "simple_peripheral_oad_offchip.h"

static signed char init_rslt;

unsigned char isPeripheralActive = 0U;

signed char s_sys_user_init_main(void)
{
    isPeripheralActive = 1U;

   init_rslt += H_CC2640R2F_UART_Init();
   init_rslt += H_CC2640R2F_IIC_Init();
//
   flash_open();
   flash_close();

    return 0;
}

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (50)

static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};

static void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

    result = mpu_run_self_test(gyro, accel);
    if (result == 0x7) {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
        printf("setting bias succesfully ......\r\n");
    }
}

void inv_dmp_init(void)
{
    inv_error_t result;
//    unsigned char accel_fsr,  new_temp = 0;
//    unsigned short gyro_rate, gyro_fsr;
//    unsigned long timestamp;
//    struct int_param_s int_param;

#ifdef COMPASS_ENABLED
    unsigned char new_compass = 0;
    unsigned short compass_fsr;
#endif

  result = mpu_init(NULL);
  if (result) {
      printf("Could not initialize gyro.\n");
  }

  if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))    //设置需要的传感器
       printf("mpu_set_sensor complete ......\r\n");

    if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //设置fifo
       printf("mpu_configure_fifo complete ......\r\n");
    if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))              //设置采集样率
       printf("mpu_set_sample_rate complete ......\r\n");
    if(!dmp_load_motion_driver_firmware())                //加载dmp固件
      printf("dmp_load_motion_driver_firmware complete ......\r\n");
    if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
       printf("dmp_set_orientation complete ......\r\n"); //设置陀螺仪方向
    if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
          DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
          DMP_FEATURE_GYRO_CAL))
       printf("dmp_enable_feature complete ......\r\n");
    if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))    //设置速率
       printf("dmp_set_fifo_rate complete ......\r\n");
    run_self_test();                          //自检
    if(!mpu_set_dmp_state(1))                 //使能
       printf("mpu_set_dmp_state complete ......\r\n");
}

#define q30  1073741824.0f

signed char get_dmp_vector_data(long *q)
{
    short gyro[3], accel[3], sensors;
//    long quat[4];
//    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    unsigned long sensor_timestamp;
    unsigned char more;
    if(dmp_read_fifo(gyro, accel, q, &sensor_timestamp, &sensors, &more))
    {
        return 1;
    }
    if (sensors & INV_WXYZ_QUAT)
    {
        return 0;
    }
    return 2;
}

signed char get_dmp_rotation_data(float* Pitch,float* Roll,float* Yaw)
{
    short gyro[3], accel[3], sensors;
    long quat[4];
    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    unsigned long sensor_timestamp;
    unsigned char more;
    if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))
    {
        return 1;
    }
    if (sensors & INV_WXYZ_QUAT)
    {
         q0=quat[0] / q30;
         q1=quat[1] / q30;
         q2=quat[2] / q30;
         q3=quat[3] / q30;
         *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
         *Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
         *Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
         return 0;
    }

    return 2;

}


signed char s_sys_user_setting_before_ble_init(void)
{
    inv_dmp_init();

    return 0;
}


signed char s_sys_user_setting_after_ble_init(void)
{
//    H_CC2640R2F_UART_RxEnable();

//    H_CC2640R2F_IIC_Test();
//    s_start_clock_util();

    return 0;
}


